/**
* @file        DREngine
* @brief       
* @version     1.0.0
* @author      Changlin.Jing 
* @date        2023/2/16 9:29
* @copyright   Copyright  2022 Langge Software Co., Ltd. All rights reserved.
*/
#ifndef LANGGEBASE_DRENGINE_HPP
#define LANGGEBASE_DRENGINE_HPP

#include <set>
#include <shared_mutex>
#include <deque>
#include "ut_symbols.h"
#include "loc_signal_type.h"
#include "LocSignalDeliverManager.hpp"
#include "../algorithm/SensorDataCache.hpp"
#include "../algorithm/StaticStateDetection.hpp"
#include "../algorithm/NaviFusionImpl.hpp"
#include "../algorithm/ESKFFusionSimple.hpp"


BEGIN_SPACE_LANGE_VDR
class DREngine{
    private:
        DREngine() = default;
        static DREngine * mInstance;
    public:
        static DREngine * GetInstance();

        void Init(const WorkState& workState);

        void Stop();

        bool Reset();

        //void OnPosSignalUpdate(std::shared_ptr<LOC_POS_SIGNAL> &posSignal) override;

        void OnSignal(const std::shared_ptr<I_LOC_SIGNAL> &posSignal);


        bool AddLocationObserver(ILocationObserver *pObserver);

        bool RemoveLocationObserver(ILocationObserver *pObserver);


        void SetConfigFilePath(std::string& filePath);

    protected:
        std::shared_ptr<LocSignalDeliverManager> mDeliverManager;
        std::shared_ptr<NaviData::BaseData> ConvertGnssSig(std::shared_ptr<LOC_POS_SIGNAL> &posSignal);
        std::shared_ptr<NaviData::BaseData> ConvertCarSig(const std::shared_ptr<LOC_CAR_SIGNAL> &carSignal);
        std::shared_ptr<NaviData::ImuData> Convert2ImuSignal(const std::shared_ptr<I_LOC_SIGNAL> &sharedPtr);

        std::set<ILocationObserver *> m_LocInfoObservers;
        std::shared_ptr<ESKFFusionSimple> mPtrNaviFusion = std::make_shared<ESKFFusionSimple>();
        std::shared_ptr<vdr::StaticStateDetection>  mPtrStaticStateDetection = std::make_shared<vdr::StaticStateDetection>(100,0.08);

        std::shared_ptr<LOC_POS_SIGNAL> GetFusionSignal(uint64_t time);
        std::int64_t mLastNotifyTimestamp = -1;
        bool mIsNeedConvert = false;
        bool hasWorkStateCheckComplete = false;
        ////////本次用到的控制参数
        //计时器回调时间设置
        int NOTIFY_INTERVAL = 100;
        std::int64_t mCurTimestampMs = -1;
        std::int64_t m_DrInitTime_ms = -1;

        std::shared_ptr<LOC_ACC_SIGNAL> mCurrAccSignal = nullptr;
        std::shared_ptr<LOC_GYRO_SIGNAL> mCurrGyroSignal = nullptr;
        std::shared_ptr<LOC_GYRO_SIGNAL> mPrevGyroSignal = nullptr;
        NaviData::ImuData cur_imu, pre_imu;

        const int MAX_SIZE_ACCPOOL = 10;
        double mTimeScale = 0.001;     // 把毫秒转换成秒
        double mImuMaxInterval = 0.02; // 0.02 秒
        std::deque<std::shared_ptr<LOC_ACC_SIGNAL>> mAccSignalPool;

    };
END_SPACE_LANGE_VDR
#endif //LANGGEBASE_DRENGINE_HPP
